#include "ShooterEncoder.h"
#include <stdio.h>



ShooterEncoder::ShooterEncoder(int channel) : Counter(channel)
{
	// Channel value is passed to the Counter constructor.
	
	for (int i = 0; i < 2; ++i)
		sampledRPSs[i] = -1.0;
	
	averageRPS = 0.0;
	
	samplerIndex = 0;
	
	lastTime = GetPPCTimestampFine();
	
	// Start the counter.
	Start();
}

ShooterEncoder::~ShooterEncoder()
{
	
}

void ShooterEncoder::UpdateAverageRPS()
{
	// Check every number in the "lastPeriod" array.
	// For every number that isn't -1, add it to the total and increase the count of valid numbers in the array.
	float sum = 0.0; 
	int count = 0;
	
	for (int i = 0; i < 2; ++i)
	{
		if (sampledRPSs[i] != -1.0)
		{
			++count;
			sum += sampledRPSs[i];
		}
	}
	
	averageRPS = ((count == 0) ? 0 : (sum / count));
}

double ShooterEncoder::GetAverageRPS()
{
	return averageRPS;
}

double ShooterEncoder::GetPPCTimestampFine() //Returns the timestamp in milliseconds
{
	// PPC system clock is 33MHz
	return niTimestamp64() / 33.0e3;
}
 
double ShooterEncoder::PIDGet()
{
	return averageRPS;
}


void ShooterEncoder::Update()
{
	// Get current period, count, and time, and calculate the delta values since last update.
	int deltaCount = Get();
	float currentTime = GetPPCTimestampFine();
	float deltaTime = currentTime - lastTime;
	
	if (deltaTime == 0)
	{
		deltaTime = 10000; // Set to some big number, because it'll result in a low RPM (what we want, since this is our first time through
	}
	
	//deltaCount ticks occured in currentTime - this->lastTime milliseconds. Multiply by 60k to get ticks/minute.
	//Divide that by 6 to get revolutions/minute, divide by 60 to get to RPS.
	float rps = deltaCount * 10000 / (60 * deltaTime);
	
	//Set the new lastTime value
	lastTime = currentTime;
	
	// Reset the count on the encoder.
	Reset();
	
	// Add one to RPM location and mod by 11, to bracket it between 0 and 11.
	(++samplerIndex) %= 2;
	
	// Add current RPM to the last 12 RPM readings.
	sampledRPSs[samplerIndex] = rps;
	
	UpdateAverageRPS();
}


